//
// Created by bourne on 2024/4/20.
//
#include "plan_env/sdf_map.h"
#include "plan_env/map_ros.h"
#include "plan_env/edt_environment.h"
#include "active_perception/perception_utils.h"
#include "active_perception/frontier_finder.h"
#include "traj_utils/planning_visualization.h"

using namespace fast_planner;

int drone_id;

shared_ptr<SDFMap> sdf_map_;
shared_ptr<EDTEnvironment> edt_environment_;
shared_ptr<FrontierFinder> frontier_finder_;
shared_ptr<PlanningVisualization> visualization_;

Eigen::Vector3d pos_;

vector<vector<Vector3d>> frontiers_;
vector<Eigen::Vector3d> viewpoints_;
vector<double> yaws_;
vector<Eigen::Vector3d> averages_;

void odomCallback(const nav_msgs::OdometryConstPtr& msg) {
  pos_[0] = msg->pose.pose.position.x;
  pos_[1] = msg->pose.pose.position.y;
  pos_[2] = msg->pose.pose.position.z;
}

void updateTimerCallback(const ros::TimerEvent& e) {
  ros::Time t1 = ros::Time::now();
  frontier_finder_->searchFrontier();
  ros::Time t2 = ros::Time::now();
  frontier_finder_->computeFrontierToVisit();
  ros::Time t3 = ros::Time::now();

  frontier_finder_->getFrontiers(frontiers_);
  frontier_finder_->getTopViewpointsInfo(pos_, viewpoints_, yaws_, averages_);

  // display frontiers
  for (int i = 0; i < frontiers_.size(); i++) {
    visualization_->drawCubes(frontiers_[i], 0.1,
       visualization_->getColor(double(i) / frontiers_.size(), 0.4), "frontier", i, 4);
  }

  // display viewpoints
  visualization_->drawSpheres(viewpoints_, 0.3, Eigen::Vector4d(1.0, 0.0, 0.0, 1.0), "viewpoints", 1, 6);

  double search_t = (t2 - t1).toSec();
  double view_point_t = (t3 - t2).toSec();
  std::cout << "[FrontierNode] drone " << drone_id << ": frontier t " << search_t
                << "s, view t: " << view_point_t << "s" << std::endl;
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "frontier_node");
  ros::NodeHandle nh("~");

  nh.param("exploration/drone_id", drone_id, -1);

  sdf_map_.reset(new SDFMap);
  sdf_map_->initMap(nh);
  edt_environment_.reset(new EDTEnvironment);
  edt_environment_->setMap(sdf_map_);
  frontier_finder_.reset(new FrontierFinder(edt_environment_, nh));
  visualization_.reset(new PlanningVisualization(nh));

  ros::Subscriber odom_sub = nh.subscribe("/odom_world", 10, odomCallback);

  ros::Timer update_timer = nh.createTimer(ros::Duration(1.0), updateTimerCallback);

  ros::spin();
  return 0;
}


